% 该程序用于计算仅含航程约束的制导律的终端时间
clc
clear
close all
global m S d2r R0 g0 rot Vs ts Vup Vdown alphamax alphamin alphaLD dAziUp dAziDown...
       dtInt dtGui N ef absBankMax Vf
d2r = pi/180;
m = 907;
S = 0.48;
R0 = 6378135;
g0 = 9.81;
Vs = sqrt(g0*R0);
ts = sqrt(R0/g0);
rot = 7.292115E-5;

Vf = 2000/Vs;
fpa0 = -1*d2r;
h0 = 72e3;
lonf = 50*d2r;
latf = 50*d2r;
hf = 30e3;

% F1
% lon0 = 6.5*d2r;
% lat0 = 7.5*d2r;
% azi0 = 31*d2r;
% F2
% lon0 = 15*d2r;
% lat0 = 3*d2r;
% azi0 = 28*d2r;
V0 = 6100/Vs;
lon0 = -2*d2r;
lat0 = 15*d2r;
azi0 = 35*d2r;
% lon0 = 7*d2r;
% lat0 = 8*d2r;
% azi0 = 30*d2r;

rad0 = (R0 + h0)/R0;
radf = (R0 + hf)/R0;
e0 = 1/rad0 - V0^2/2;
ef = 1/radf - Vf^2/2;
Stogo0 = acos(sin(lat0)*sin(latf) + cos(lat0)*cos(latf)*cos(lonf-lon0));
alphamax = 20*d2r;
alphaLD = 12*d2r;
alphamin = 5*d2r;
absBankMax = 60*pi/180;
Vup = 5900/Vs;
Vdown = 5000/Vs;
dAziUp = 10*d2r;% 航向角误差走廊
dAziDown = -10*d2r;
dtInt = 0.1/ts; % 积分周期
dtGui = 1/ts; % 制导周期
N = 1000;
p = 1;
state(p,:) = [rad0, lon0, lat0, V0, fpa0, azi0];
statef = [radf, lonf, latf, Vf, nan, nan];
time(p,1) = 0;
ctrl(p,:)= [20*d2r, 0, 1]; % 攻角，倾侧角幅值，倾侧角符号
auxState(p,:) = [e0 Stogo0 0 0 0 0 0 0 0 0 0]; % e stogo bankbase bankf dAzi iter F G dQ n q
k = dtGui/dtInt;
fpaflag = false(1);
while 1
    statet = RK4(@ReentryDynamic, state(p,:), ctrl(p,:), dtInt);
    rad = statet(1);
    V = statet(4);
    fpa = statet(5);
    if V > Vup
        alphar = alphamax;
    elseif V > Vdown
        alphar = (alphamax - alphaLD)*(V - Vdown)/(Vup - Vdown) + alphaLD;
    else
        alphar = alphaLD;
    end
    if k <= 0
        if fpaflag
            [ctrl_temp, aux] = GuidanceSLoop(time(p,1), state(p,:),...
                statef, ctrl(p,:), auxState(p,2:11), alphar);
            alpha = limitRate(ctrl_temp(1), ctrl(p,1), dtGui*ts, 2*pi/180);
            bank = limitRate(ctrl_temp(2), ctrl(p,2), dtGui*ts, 10*pi/180);
            ctrl_temp = [alpha bank ctrl_temp(3)];
        else
            ctrl_temp = [alphar 0 1];
        end
    else
        ctrl_temp = ctrl(p,:);
    end
    ctrl(p+1,:) = ctrl_temp;
    state(p+1,:) = statet;
    time(p+1,1) = time(p,1) + dtInt;
    if fpa > 0
        fpaflag = true(1);
    end
    e = 1/rad - V^2/2;
    [dQ, n, q] = calpathVar(alphar, ctrl_temp(1), rad, V);
    auxState(p+1,:) = [e Stogo0 30*d2r 30*d2r 0 0 0 0 dQ n q];
    if exist('aux', 'var')
        auxState(p+1,2:11) = aux;
    end
    s = auxState(p+1,2);
    p = p+1;
    k = k-1;
    if e >= ef
        break;
    end
end
%% plot
t = ts*time;
alt = (state(:, 1) - 1)*R0/1000;
lon = state(:, 2)*180/pi;
lat = state(:, 3)*180/pi;
V = state(:, 4)*Vs;
fpa = state(:, 5)*180/pi;
azi = state(:, 6)*180/pi;
alpha = ctrl(:, 1)*180/pi;
absBank = ctrl(:, 2)*180/pi;
signOfBank = ctrl(:, 3);
bank = absBank.*signOfBank;
% e stogo bankbase bankf dAzi iter F G
bankbase = auxState(:, 3)*180/pi;
bankf = auxState(:, 4)*180/pi;
iter = auxState(:, 6);
Serror = auxState(:, 7);
Terror = auxState(:, 8);
dQ = auxState(:, 9);
n = auxState(:, 10);
q = auxState(:, 11);

f = figure('Name', '三维轨迹');
plot3(lon, lat, alt, 'linewidth', 2);
xlabel('Longitude');ylabel('Latitude');zlabel('Altitude/km');
grid on
saveas(f, f.Name, 'svg')

f = figure('Name', '地面轨迹');
plot(lon, lat, 'linewidth', 1.5);
xlabel('经度/(deg)'); ylabel('纬度/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '高度-时间历程');
plot(t, alt, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('高度/km');
saveas(f, f.Name, 'svg')

f = figure('Name', '速度-时间历程');
plot(t, V, 'linewidth', 1.5);
xlabel('时间/s)'); ylabel('速度/(m·s^{-1}）');
saveas(f, f.Name, 'svg')

f = figure('Name', '航迹角-时间历程');
plot(t, fpa, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('航迹角/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '航向角-时间历程');
plot(t, azi, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('航向角/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '攻角-时间历程');
plot(t, alpha, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('攻角/(deg)');
saveas(f, f.Name, 'svg')

f = figure('Name', '倾侧角-时间历程');
plot(t, bank, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('倾侧角/(deg)');
saveas(f, f.Name, 'svg')

figure
plot(t, signOfBank, 'o');
xlabel('时间/s'); ylabel('Sign Of Bank Angle (deg)');

figure
plot(t, bankbase, t, bankf, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('bankprofile (deg)');
legend('bankc', 'bankf');

figure
plot(t, iter, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('iterations (deg)');

figure
plot(t, Serror, t, Terror, 'linewidth', 1.5);
xlabel('时间/s'); ylabel('error');
legend('Serror', 'Terror');

figure
subplot(3,1,1)
plot(t, dQ, 'linewidth', 1.5);
ylabel("热流/(kW·m^2）");
subplot(3,1,2)
plot(t, n, 'linewidth', 1.5);
ylabel("过载/g");
subplot(3,1,3)
plot(t, q/1000, 'linewidth', 1.5);
ylabel("动压/kPa");
xlabel('时间/s');